#ifndef ROS_UDP_COMM_LOW_HPP
#define ROS_UDP_COMM_LOW_HPP

#include <ros/ros.h>
#include <unitree_legged_msgs/LowCmd.h>
#include <unitree_legged_msgs/LowState.h>
#include "stair_ctrl/UdpState.h"
#include <std_msgs/UInt8.h>
#include "unitree_legged_sdk/unitree_legged_sdk.h"

#include "stair_ctrl/convert.h"

#include <iostream>
#include <array>

using namespace UNITREE_LEGGED_SDK;

class rosUdpCommLow
{
private:
    UDP low_udp;
    Safety safe;

    LowCmd low_cmd = {0};
    LowState low_state = {0};

    ros::Subscriber sub_low;
    ros::Subscriber sub_reset_state;

    ros::Publisher pub_low;
    ros::Publisher pub_udp_state;

    ros::WallTimer timer;

    unitree_legged_msgs::LowState low_state_ros;
    stair_ctrl::UdpState udp_state_ros;

public:
    rosUdpCommLow(ros::NodeHandle *nh, 
                  int rate = 500,
                  std::string low_cmd_topic = "/stair_ctrl/low_cmd",
                  std::string low_state_topic = "low_state",
                  std::string udp_state_topic = "udp_state",
                  std::string reset_topic = "/stair_ctrl/reset") : low_udp(LOWLEVEL, 8090, "192.168.123.10", 8007),
                                                                   safe(LeggedType::Go1)
    {
        auto hints = ros::TransportHints().tcpNoDelay();
        low_udp.InitCmdData(low_cmd);

        pub_low = nh->advertise<unitree_legged_msgs::LowState>(low_state_topic, 100);
        pub_udp_state = nh->advertise<stair_ctrl::UdpState>(udp_state_topic, 100);
        sub_low = nh->subscribe(low_cmd_topic, 10, &rosUdpCommLow::lowCmdCallback, this, hints);
        sub_reset_state = nh->subscribe(reset_topic, 10, &rosUdpCommLow::resetCallback, this, hints);
        
        timer = nh->createWallTimer(ros::WallDuration(1.0 / rate), &rosUdpCommLow::periodicCommCallback, this);
    }

    void lowCmdCallback(const unitree_legged_msgs::LowCmd::ConstPtr &msg)
    {
        low_cmd = rosMsg2Cmd(msg);

        low_udp.SetSend(low_cmd);
    }

    void resetCallback(const std_msgs::UInt8::ConstPtr &msg)
    {
        std::cout << "get reset code: " << (int)msg->data << std::endl;
        if (msg->data == 233)
        {
            low_udp.InitCmdData(low_cmd);
        }
    }

    void lowUdpCommUpdate()
    {
        low_udp.Send();
        low_udp.Recv();
        low_udp.GetRecv(low_state);
    }

    void lowUdpStateUpdate()
    {
        low_state_ros = state2rosMsg(low_state);
        pub_low.publish(low_state_ros);

        udpStateToRosMsg();
        pub_udp_state.publish(udp_state_ros);
    }

    void udpStateToRosMsg()
    {
        udp_state_ros = stair_ctrl::UdpState();
        udp_state_ros.TotalCount = low_udp.udpState.TotalCount;
        udp_state_ros.SendCount = low_udp.udpState.SendCount;
        udp_state_ros.RecvCount = low_udp.udpState.RecvCount;
        udp_state_ros.SendError = low_udp.udpState.SendError;
        udp_state_ros.FlagError = low_udp.udpState.FlagError;
        udp_state_ros.RecvCRCError = low_udp.udpState.RecvCRCError;
        udp_state_ros.RecvLoseError = low_udp.udpState.RecvLoseError;
    }

    void periodicCommCallback(const ros::WallTimerEvent& event)
    {
        lowUdpCommUpdate();
        lowUdpStateUpdate();
    }

    void start()
    {
        ros::spin();
    }

    // void start(int state_rate)
    // {
    //     ros::Rate loop_rate(state_rate);

    //     // main loop
    //     while (ros::ok())
    //     {
    //         lowUdpCommUpdate();
    //         lowUdpStateUpdate();

    //         ros::spinOnce();
    //         loop_rate.sleep();
    //     }
    // }
};

#endif